/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.BridgeArmStop;

/**
 *
 * @author Driver
 */
public class BridgeArm extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    private Jaguar bridgeArm_Jag;
    public Servo bridgeArmLock;

    private double locked = 1.0;
    private double unlocked = -1.0;

    public BridgeArm() {
        super("BridgeArm");

        bridgeArm_Jag = new Jaguar(RobotMap.BRIDGE_PUSHER);
        bridgeArmLock = new Servo(RobotMap.BRIDGE_ARM_LOCK_MODULE, RobotMap.BRIDGE_ARM_LOCK);
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new BridgeArmStop());
    }

    public void bridgeArmLock() {
        bridgeArmLock.set(locked);
    }

    public void bridgeArmUnlock() {
        bridgeArmLock.set(unlocked);
    }

    public void lockGetCurPosition() {
        SmartDashboard.putDouble("Lock", bridgeArmLock.get());
    }

    public void bridgeArmStop()
    {
        bridgeArm_Jag.set(0.0);
    }

    public void raiseArm() {
        int flipped = 1;

        if(RobotMap.BRIDGE_PUSHER_REVERSED) {
            flipped = -1;
        }

        bridgeArm_Jag.set((RobotMap.BRIDGE_ARM_SPEED * flipped));
    }

    public void lowerArm() {
        int flipped = 1;

        if(RobotMap.BRIDGE_PUSHER_REVERSED) {
            flipped = -1;
        }

        bridgeArm_Jag.set(-(RobotMap.BRIDGE_ARM_SPEED * flipped));
    }

    public void setSpeed(double speed) {
        int flipped = 1;

        if(RobotMap.BRIDGE_PUSHER_REVERSED) {
            flipped = -1;
        }

        bridgeArm_Jag.set(1.0 * flipped);
    }
}